home *** CD-ROM | disk | FTP | other *** search
/ Linux Cubed Series 8: LINUX Games / Linux Cubed Series 8 - LINUX Games.iso / games / video / fly8111-.000 / fly8111- / fly8 / dr.c < prev    next >
C/C++ Source or Header  |  1979-12-31  |  4KB  |  182 lines
  1. /* ------------------------------------ dr.c -------------------------------- */
  2.  
  3. /* This is part of the flight simulator 'fly8'.
  4.  * Author: Eyal Lebedinsky (eyal@ise.canberra.edu.au).
  5. */
  6.  
  7. /* Dead reckoning matters. Will be used for the implementation of the
  8.  * DIS protocol.
  9. */
  10.  
  11. #include "fly.h"
  12.  
  13.  
  14. Given any Entity, we have:
  15.  
  16.     Entity linear velocity (v:V0) (World)
  17.     Entity location (v:P0) (World)
  18.     Entity orientation (m:R0) (World->Body)
  19.  
  20.     Dead Reckoning method
  21.     Dead Reckoning Linear acceleration (v:A0) (World)
  22.     Dead Reckoning Angular velocity (v:W) (Body)
  23.  
  24.     Delta time (dT)
  25.  
  26. The Dead Reckoning process then works as followes.
  27.  
  28.     w = sqrt (Wx**2+Wy**2+Wz**2)
  29.     Phi = w * dT
  30.  
  31.     A = W/w
  32.  
  33.     I = Identity matrix
  34.  
  35.     AX =      0    -Az     Ay
  36.          Az      0    -Ax
  37.         -Ay     Ax      0
  38.  
  39.     A*At =    Ax*Ax    Ax*Ay    Ax*Az
  40.         Ay*Ax    Ay*Ay    Ay*Az
  41.         Az*Ax    Az*Ay    Az*Az
  42.  
  43.     DR = I*cos(Phi) - AX*sin(Phi) + A*At*(1-cos(Phi)
  44.  
  45. If the dead reckoning parameters are given in body axis then:
  46.  
  47.     Vb = Velocity in body axis (given)
  48.     AB = Linear Acceleration in body axis (given)
  49.  
  50. Ab is the Effective Linear Acceleration in body axis:
  51.  
  52.     Ab = AB - AX*Vb
  53.  
  54.     dw = w*dT
  55.     s = sin(dw)
  56.     c = cos(dw)
  57.  
  58.  
  59.          dw-s        s     1-c
  60.     R1 = ----*W*Wt + -*I + ----*OM
  61.          w**3        w     w**2
  62.  
  63.  
  64.          1/2*dw**2-c-dw*s+1        c+dw*s-1     s-dw*c
  65.     R2 = ------------------*W*Wt + --------*I + ------*OM
  66.          w**4                      w**2         w**3
  67.  
  68.  
  69. DRM_STATIC:
  70.     All static, no change.
  71.  
  72. DRM_FPW:
  73.     P = P0 + V0 * dT
  74.  
  75. DRM_RPW:
  76.     P = P0 + V0 * dT
  77.     R = DR * R0
  78.     EA = EA0 + EAr*dT
  79.  
  80. DRM_RVW:
  81.     P = P0 + V0 * dT + 1/2 * A0 * dT**2
  82.     R = DR * R0
  83.     EA = EA0 + EAr*dT
  84.  
  85. DRM_FVW:
  86.     P = P0 + V0 * dT + 1/2 * A0 * dT**2
  87.  
  88. DRM_FPB:
  89.     P = P0 +R**-1*(R1*Vb)
  90.  
  91. DRM_RPB:
  92.     P = P0 +R**-1*(R1*Vb)
  93.     R = DR * R0
  94.     EA extracted from R
  95.  
  96. DRM_RVB:
  97.     P = P0 +R**-1*(R1*Vb + R2*Ab)
  98.     R = DR * R0
  99.     EA extracted from R
  100.  
  101. DRM_FVB:
  102.     P = P0 +R**-1*(R1*Vb + R2*Ab)
  103.  
  104.  
  105.  
  106. /*
  107.  * The relationship between Fly8 and DIS is this:
  108.  *
  109.  *    Fly8 x        DIS y
  110.  *    Fly8 a[x]    DIS Theta
  111.  *    Fly8 y        DIS x
  112.  *    Fly8 a[y]    DIS Phi
  113.  *    Fly8 z        DIS -z
  114.  *    Fly8 a[z]    DIS -Psi
  115.  *
  116.  * So, suppose we keep the names as:
  117.  *
  118.  *    x pitch
  119.  *    y Roll
  120.  *    z heading
  121.  *
  122.  * This is the Fly8 orientation matrix:
  123.  *
  124.  *    .[0]        .[1]        .[2]
  125.  * [0].    cy*cz-sy*sz*sx    cy*sz+sy*cz*sx    -sy*cx    [0].
  126.  * [1].    -cx*sz        cx*cz        sx    [1].
  127.  * [2].    sy*cz+cy*sz*sx    sy*sz-cy*cz*sx    cy*cx    [2].
  128.  *    .[0]        .[1]        .[2]
  129.  *
  130.  * First we apply
  131.  *
  132.  *    z -> -z (heading)
  133.  *
  134.  * so we apply: sz -> -sz)
  135.  *
  136.  *    .[0]        .[1]        .[2]
  137.  * [0].    cy*cz+sy*sz*sx    -cy*sz+sy*cz*sx    -sy*cx    [0].
  138.  * [1].    cx*sz        cx*cz        sx    [1].
  139.  * [2].    sy*cz-cy*sz*sx    -sy*sz-cy*cz*sx    cy*cx    [2].
  140.  *    .[0]        .[1]        .[2]
  141.  *
  142.  * Then we rename the axes:
  143.  *
  144.  *    x -> y
  145.  *    y -> x
  146.  *
  147.  * so we swap rows 0 and 1, and columns 0 and 1:
  148.  *
  149.  *    .[0]        .[1]        .[2]
  150.  * [0].    cx*cz        cx*sz        sx    [1].
  151.  * [0].    -cy*sz+sy*cz*sx    cy*cz+sy*sz*sx    -sy*cx    [0].
  152.  * [2].    -sy*sz-cy*cz*sx    sy*cz-cy*sz*sx    cy*cx    [2].
  153.  *    .[0]        .[1]        .[2]
  154.  *
  155.  * Now we apply
  156.  *
  157.  *    z -> -z (the axis)
  158.  *
  159.  * so we negate row 2 and column 2:
  160.  *
  161.  *    .[0]        .[1]        .[2]
  162.  * [0].    cx*cz        cx*sz        -sx    [1].
  163.  * [0].    -cy*sz+sy*cz*sx    cy*cz+sy*sz*sx    sy*cx    [0].
  164.  * [2].    +sy*sz+cy*cz*sx    -sy*cz+cy*sz*sx    cy*cx    [2].
  165.  *    .[0]        .[1]        .[2]
  166.  *
  167.  * That's it. Some cosmetics:
  168.  *
  169.  *    .[0]        .[1]        .[2]
  170.  * [0].    cx*cz        cx*sz        -sx    [1].
  171.  * [0].    sy*cz*sx-cy*sz    sy*sz*sx+cy*cz    sy*cx    [0].
  172.  * [2].    cy*cz*sx+sy*sz    cy*sz*sx-sy*cz    cy*cx    [2].
  173.  *    .[0]        .[1]        .[2]
  174.  *
  175.  * However, this is a big lie. The two formulaes (the one above and
  176.  * the one in appendix B) appear identical, but FLy8 keeps the b->w
  177.  * matrix while DIS defined the w->b! Luckily, Fly8 also uses row vectors
  178.  * when DIS uses column vectors so we are OK (the inverse is here the
  179.  * transpose).
  180. */
  181.  
  182.